#include "../globals/globals.h"
#include "ActionUnit.h"

vector<ActionUnit *>
    ActionUnit::vec_ptr_all_instacnes;  // 定义所有产生的实例的指针
bool ActionUnit::b_auto_relaxing_on = true;

//////////////////////////////////////////////////////////////////////////////////////////////////////////
// 获取含有左右前后的马达名字
char *ActionUnit::get_motor_name_with_side_suffix(int motor_index) {
  static char name_str[20];
  if (get_motor_count() >= 2) {
    // 底盘: 单元名字中含有"chassis" 和"chassis"
    if (get_name().find("chassis") != string::npos ||
        get_name().find("chassis") != string::npos) {
      // 底盘前后中
      if (motor_index == 0)
        sprintf(name_str, "%sf", get_name_c_str());
      else if (motor_index == 1)
        sprintf(name_str, "%sb", get_name_c_str());
      else if (motor_index == 2)
        sprintf(name_str, "%sm", get_name_c_str());
      else
        sprintf(name_str, "%s%d", get_name_c_str(), motor_index);
    } else {
      // 其他左右
      if (motor_index == 0)
        sprintf(name_str, "%sl", get_name_c_str());
      else if (motor_index == 1)
        sprintf(name_str, "%sr", get_name_c_str());
      else if (motor_index == 2)
        sprintf(name_str, "%sm", get_name_c_str());
      else
        sprintf(name_str, "%s_%d", get_name_c_str(), motor_index);
    }
  } else  // 1个马达
    sprintf(name_str, "%s", get_name_c_str());
  return name_str;
}
/////////////////////////////////////////////////////////////////////////////////////////////////
void ActionUnit::all_units_reset_enc(string except_au_1_name,
                                     string except_au_2_name,
                                     string except_au_3_name,
                                     string except_au_4_name) {
  for (auto pau : vec_ptr_all_instacnes) {
    if (pau->unit_name != except_au_1_name &&
        pau->unit_name != except_au_2_name &&
        pau->unit_name != except_au_3_name &&
        pau->unit_name != except_au_4_name)
      pau->reset_enc();
  }
}

void ActionUnit::all_units_stop() {
  for (auto pau : vec_ptr_all_instacnes) {
    if (pau->is_act_enc_running() == false)
      pau->mtr_stop();  // 只停马达
    else
      pau->stop();  // 停线程和马达
  }
}

void ActionUnit::all_units_stop(brakeType brake_type) {
  for (auto pau : vec_ptr_all_instacnes) {
    if (pau->is_act_enc_running() == false)
      pau->mtr_stop(brake_type);  // 只停马达
    else
      pau->stop(brake_type);  // 停线程和马达
  }
}
////////////////////////////////////////////////////////////////////
double ActionUnit::get_watt()  // 合计功率
{
  double watt = 0;
  for (int k = 0; k < vec_ptr_motors.size(); k++)  // 所有马达
    watt += vec_ptr_motors[k]->power();
  return watt;
}

void ActionUnit::reset_enc() { mtr_group.resetPosition(); }

int ActionUnit::get_installed_motor_index_with_default(int default_index) {
  if (vec_ptr_motors[default_index]->installed())
    return default_index;
  else  // 默认马达号未接线，从0找到最大值
  {
    for (int k = 0; k < mtr_group.count(); k++) {
      if (vec_ptr_motors[k]->installed()) return k;
    }
    return 0;
  }
}

double ActionUnit::get_enc(int motor_index, rotationUnits unit) {
  if (motor_index > get_motor_count() - 1 || motor_index < 0) return -1;
  wait(1);  // 短暂延时，防止连续读取本函数,用于cout打印显示不出来
  return vec_ptr_motors[get_installed_motor_index_with_default(motor_index)]
      ->position(unit);
}

double ActionUnit::get_actual_velocity(int motor_index, velocityUnits units_v) {
  if (motor_index > get_motor_count() - 1 || motor_index < 0) return -1;
  return vec_ptr_motors[get_installed_motor_index_with_default(motor_index)]
      ->velocity(units_v);
}

double ActionUnit::get_actual_velocity_pct(int motor_index) {
  return vec_ptr_motors[get_installed_motor_index_with_default(motor_index)]
      ->velocity(velocityUnits::pct);
}

/////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////
void ActionUnit::mtr_set_voltage(double pct_voltage) {
  set_speed_pct = 0;
  set_voltage_pct = pct_voltage;

  if (pct_voltage == 0)
    mtr_stop();  // 根据情况停转
  else
    mtr_group.spin(directionType::fwd, 12.8 * pct_voltage / 100.0,
                   voltageUnits::volt);
}

void ActionUnit::set_voltage(double pct_voltage) {
  stop_act_enc();
  mtr_set_voltage(pct_voltage);
}

void ActionUnit::set_voltage(double voltage, voltageUnits unit) {
  if (unit == voltageUnits::mV)
    set_voltage_pct = voltage / 1000.0 / 12.8 * 100;
  else
    set_voltage_pct = voltage / 12.8 * 100;
  set_voltage(set_voltage_pct);
}

void ActionUnit::set_speed(double speed, velocityUnits unit) {
  set_voltage_pct = 0;
  if (unit == velocityUnits::pct)
    set_speed_pct = speed;
  else
    set_speed_pct = 0;  //*****其他单位直接set_speed_pct为0不想写了，就这样吧~

  stop_act_enc();
  if (speed == 0)
    mtr_stop();
  else
    mtr_group.spin(directionType::fwd, speed, unit);
}

void ActionUnit::mtr_stop() {
  set_voltage_pct = 0;
  set_speed_pct = 0;
  if (b_relaxing_static)
    mtr_group.stop(brakeType::coast);
  else
    mtr_group.stop(set_brake_type);
}

void ActionUnit::mtr_stop(brakeType brake_type) {
  set_voltage_pct = 0;
  set_speed_pct = 0;
  mtr_group.stop(brake_type);
}

void ActionUnit::stop() {  // 停止后台线程和马达
  stop_act_enc();
  mtr_stop();
}

void ActionUnit::stop(brakeType brake_type) {  // 停止后台线程和马达
  stop_act_enc();
  mtr_stop(brake_type);
}
/////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////
double ActionUnit::get_set_power_pct() {  // 速度或者电压
  if (set_voltage_pct != 0) return set_voltage_pct;
  if (set_speed_pct != 0) return set_speed_pct;
  return 0;
}

void ActionUnit::set_stopping(brakeType brake_type) {
  set_brake_type = brake_type;
  mtr_group.setStopping(set_brake_type);
}

bool ActionUnit::b_relaxing_static = true;

void ActionUnit::brake_stop() { mtr_stop(brakeType::brake); }
void ActionUnit::coast_stop() { mtr_stop(brakeType::coast); }
void ActionUnit::hold_stop() { mtr_stop(brakeType::hold); }

timer ActionUnit::t_working;
// 静态函数,用以控制默认stop()函数,所有实例的stop()均生效
void ActionUnit::set_all_units_motor_stop_status_on_relaxing(
    bool relaxing_condition) {
  if (relaxing_condition && !b_relaxing_static) b_relaxing_static = true;
  ActionUnit::all_units_stop();  // 所有运动单元停转
}

void ActionUnit::set_all_units_motor_stop_status_on_working(
    bool working_condition) {
  if (working_condition) {
    t_working.reset();  // 在工作,则不断清零
    if (b_relaxing_static) b_relaxing_static = false;
  }

  // 长时间没清零，则未进入工作状态，自动休息
  if (b_auto_relaxing_on && t_working.time() > time_auto_relaxing)
    set_all_units_motor_stop_status_on_relaxing();
}
